<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN"
                "http://www.w3.org/TR/REC-html40/loose.dtd">
<html>
<head>
  <title>Description of ekf</title>
  <meta name="keywords" content="ekf">
  <meta name="description" content="EKF  Extended Kalman Filter">
  <meta http-equiv="Content-Type" content="text/html; charset=iso-8859-1">
  <meta name="generator" content="m2html &copy; 2003 Guillaume Flandin">
  <meta name="robots" content="index, follow">
  <link type="text/css" rel="stylesheet" href="../../m2html.css">
</head>
<body>
<a name="_top"></a>
<div><a href="../../menu.html">Home</a> &gt;  <a href="#">ReBEL-0.2.7</a> &gt; <a href="#">core</a> &gt; ekf.m</div>

<!--<table width="100%"><tr><td align="left"><a href="../../menu.html"><img alt="<" border="0" src="../../left.png">&nbsp;Master index</a></td>
<td align="right"><a href="menu.html">Index for .\ReBEL-0.2.7\core&nbsp;<img alt=">" border="0" src="../../right.png"></a></td></tr></table>-->

<h1>ekf
</h1>

<h2><a name="_name"></a>PURPOSE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>EKF  Extended Kalman Filter</strong></div>

<h2><a name="_synopsis"></a>SYNOPSIS <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>function [xh, Px, pNoise, oNoise, InternalVariablesDS] = ekf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS) </strong></div>

<h2><a name="_description"></a>DESCRIPTION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre class="comment"> EKF  Extended Kalman Filter

   [xh, Px, pNoise, oNoise, InternalVariablesDS] = ekf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)

   This filter assumes the following standard state-space model:

     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]
     y(k) = hfun[x(k),n(k),U2(k)]

   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state
   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the
   system.

   INPUT
         state                state mean at time k-1          ( xh(k-1) )
         Pstate               state covariance at time k-1    ( Px(k-1) )
         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')
         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')
         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )
         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )
         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )
         InferenceDS          inference data structure generated by GENINFDS function.

   OUTPUT
         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )
         Px                   state covariance
         pNoise               process noise data structure     (possibly updated)
         oNoise               observation noise data structure (possibly updated)

         InternalVariablesDS  (optional) internal variables data structure
           .xh_                  predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )
           .Px_                  predicted state covariance
           .yh_                  predicted observation ( E[y(k)|Y(k-1)] )
           .inov                 inovation signal
           .Pinov                inovation covariance
           .KG                   Kalman gain

   Copyright (c) Oregon Health &amp; Science University (2006)

   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
   academic use only (see included license file) and can be obtained from
   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the
   software should contact rebel@csee.ogi.edu for commercial licensing information.

   See LICENSE (which should be part of the main toolkit distribution) for more
   detail.</pre></div>

<!-- crossreference -->
<h2><a name="_cross"></a>CROSS-REFERENCE INFORMATION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
This function calls:
<ul style="list-style-image:url(../../matlabicon.gif)">
</ul>
This function is called by:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="../.././ReBEL-0.2.7/examples/joint_estimation/demje1.html" class="code" title="">demje1</a>	DEMJE1  Demonstrate joint estimation on a 2nd order LTI system.</li><li><a href="../.././ReBEL-0.2.7/examples/joint_estimation/demje2.html" class="code" title="">demje2</a>	DEMJE2 Demonstrate nonlinear time series joint estimation for Mackey-Glass chaotic time series</li><li><a href="../.././ReBEL-0.2.7/examples/parameter_estimation/dempe1.html" class="code" title="">dempe1</a>	DEMPE1  Demonstrate parameter estimation on a simple 2nd order LTI system.</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse1.html" class="code" title="">demse1</a>	DEMSE1  Demonstrate state estimation on a simple 2nd order LTI system.</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse2.html" class="code" title="">demse2</a>	DEMSE2  Demonstrate state estimation on a simple scalar nonlinear (time variant) problem</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse3.html" class="code" title="">demse3</a>	DEMSE3  Demonstrate nonlinear time series state estimation for Mackey-Glass chaotic time series</li></ul>
<!-- crossreference -->


<h2><a name="_source"></a>SOURCE CODE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre>0001 <a name="_sub0" href="#_subfunctions" class="code">function [xh, Px, pNoise, oNoise, InternalVariablesDS] = ekf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)</a>
0002 
0003 <span class="comment">% EKF  Extended Kalman Filter</span>
0004 <span class="comment">%</span>
0005 <span class="comment">%   [xh, Px, pNoise, oNoise, InternalVariablesDS] = ekf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)</span>
0006 <span class="comment">%</span>
0007 <span class="comment">%   This filter assumes the following standard state-space model:</span>
0008 <span class="comment">%</span>
0009 <span class="comment">%     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]</span>
0010 <span class="comment">%     y(k) = hfun[x(k),n(k),U2(k)]</span>
0011 <span class="comment">%</span>
0012 <span class="comment">%   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state</span>
0013 <span class="comment">%   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the</span>
0014 <span class="comment">%   system.</span>
0015 <span class="comment">%</span>
0016 <span class="comment">%   INPUT</span>
0017 <span class="comment">%         state                state mean at time k-1          ( xh(k-1) )</span>
0018 <span class="comment">%         Pstate               state covariance at time k-1    ( Px(k-1) )</span>
0019 <span class="comment">%         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')</span>
0020 <span class="comment">%         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')</span>
0021 <span class="comment">%         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )</span>
0022 <span class="comment">%         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )</span>
0023 <span class="comment">%         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )</span>
0024 <span class="comment">%         InferenceDS          inference data structure generated by GENINFDS function.</span>
0025 <span class="comment">%</span>
0026 <span class="comment">%   OUTPUT</span>
0027 <span class="comment">%         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )</span>
0028 <span class="comment">%         Px                   state covariance</span>
0029 <span class="comment">%         pNoise               process noise data structure     (possibly updated)</span>
0030 <span class="comment">%         oNoise               observation noise data structure (possibly updated)</span>
0031 <span class="comment">%</span>
0032 <span class="comment">%         InternalVariablesDS  (optional) internal variables data structure</span>
0033 <span class="comment">%           .xh_                  predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )</span>
0034 <span class="comment">%           .Px_                  predicted state covariance</span>
0035 <span class="comment">%           .yh_                  predicted observation ( E[y(k)|Y(k-1)] )</span>
0036 <span class="comment">%           .inov                 inovation signal</span>
0037 <span class="comment">%           .Pinov                inovation covariance</span>
0038 <span class="comment">%           .KG                   Kalman gain</span>
0039 <span class="comment">%</span>
0040 <span class="comment">%   Copyright (c) Oregon Health &amp; Science University (2006)</span>
0041 <span class="comment">%</span>
0042 <span class="comment">%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for</span>
0043 <span class="comment">%   academic use only (see included license file) and can be obtained from</span>
0044 <span class="comment">%   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the</span>
0045 <span class="comment">%   software should contact rebel@csee.ogi.edu for commercial licensing information.</span>
0046 <span class="comment">%</span>
0047 <span class="comment">%   See LICENSE (which should be part of the main toolkit distribution) for more</span>
0048 <span class="comment">%   detail.</span>
0049 
0050 <span class="comment">%=============================================================================================</span>
0051 
0052 Xdim  = InferenceDS.statedim;                                <span class="comment">% state dimension</span>
0053 Odim  = InferenceDS.obsdim;                                  <span class="comment">% observation dimension</span>
0054 U1dim = InferenceDS.U1dim;                                   <span class="comment">% exogenous input 1 dimension</span>
0055 U2dim = InferenceDS.U2dim;                                   <span class="comment">% exogenous input 2 dimension</span>
0056 Vdim  = InferenceDS.Vdim;                                    <span class="comment">% process noise dimension</span>
0057 Ndim  = InferenceDS.Ndim;                                    <span class="comment">% observation noise dimension</span>
0058 
0059 NOV = size(obs,2);                                           <span class="comment">% number of input vectors</span>
0060 
0061 <span class="comment">%------------------------------------------------------------------------------------------------------------------</span>
0062 <span class="comment">%-- ERROR CHECKING</span>
0063 
0064 <span class="keyword">if</span> (nargin ~= 8) error(<span class="string">' [ ekf ] Not enough input arguments.'</span>); <span class="keyword">end</span>
0065 
0066 <span class="keyword">if</span> (Xdim~=size(state,1)) error(<span class="string">' [ ekf ] Prior state dimension differs from InferenceDS.statedim'</span>); <span class="keyword">end</span>
0067 <span class="keyword">if</span> (Xdim~=size(Pstate,1)) error(<span class="string">' [ ekf ] Prior state covariance dimension differs from InferenceDS.statedim'</span>); <span class="keyword">end</span>
0068 <span class="keyword">if</span> (Odim~=size(obs,1)) error(<span class="string">' [ ekf ] Observation dimension differs from InferenceDS.obsdim'</span>); <span class="keyword">end</span>
0069 <span class="keyword">if</span> U1dim
0070   [dim,nop] = size(U1);
0071   <span class="keyword">if</span> (U1dim~=dim) error(<span class="string">' [ ekf ] Exogenous input U1 dimension differs from InferenceDS.U1dim'</span>); <span class="keyword">end</span>
0072   <span class="keyword">if</span> (dim &amp; (NOV~=nop)) error(<span class="string">' [ ekf ] Number of observation vectors and number of exogenous input U1 vectors do not agree!'</span>); <span class="keyword">end</span>
0073 <span class="keyword">end</span>
0074 <span class="keyword">if</span> U2dim
0075   [dim,nop] = size(U2);
0076   <span class="keyword">if</span> (U2dim~=dim) error(<span class="string">' [ ekf ] Exogenous input U2 dimension differs from InferenceDS.U2dim'</span>); <span class="keyword">end</span>
0077   <span class="keyword">if</span> (dim &amp; (NOV~=nop)) error(<span class="string">' [ ekf ] Number of observation vectors and number of exogenous input U2 vectors do not agree!'</span>); <span class="keyword">end</span>
0078 <span class="keyword">end</span>
0079 
0080 <span class="comment">%------------------------------------------------------------------------------------------------------------------</span>
0081 
0082 xh   = zeros(Xdim,NOV);
0083 xh_  = zeros(Xdim,NOV);
0084 yh_  = zeros(Odim,NOV);
0085 inov = zeros(Odim,NOV);
0086 
0087 <span class="keyword">if</span> (U1dim==0), UU1=zeros(0,1); <span class="keyword">end</span>
0088 <span class="keyword">if</span> (U2dim==0), UU2=zeros(0,1); <span class="keyword">end</span>
0089 
0090 <span class="comment">%--------------------------------------- Loop over all input vectors --------------------------------------------</span>
0091 <span class="keyword">for</span> i=1:NOV,
0092 
0093     <span class="keyword">if</span> (U1dim~=0)
0094         UU1 = U1(:,i);            <span class="comment">% get exogenous input</span>
0095     <span class="keyword">end</span>
0096     <span class="keyword">if</span> (U2dim~=0)
0097         UU2 = U2(:,i);            <span class="comment">% get exogenous input</span>
0098     <span class="keyword">end</span>
0099 
0100     <span class="comment">%------------------------------------------------------</span>
0101     <span class="comment">% TIME UPDATE</span>
0102 
0103     <span class="comment">% linearize FFUN</span>
0104     [A,G] = InferenceDS.linearize( InferenceDS, state, pNoise.mu, oNoise.mu, UU1, UU2, <span class="string">'A'</span>,<span class="string">'G'</span>);
0105 
0106     xh_(:,i) = InferenceDS.ffun( InferenceDS, state, pNoise.mu, UU1);
0107     Px_      = A*Pstate*A' + G*pNoise.cov*G';
0108 
0109     <span class="comment">% MEASUREMENT UPDATE</span>
0110 
0111     <span class="comment">% linearize HFUN</span>
0112     [C,H] = InferenceDS.linearize( InferenceDS, xh_(:,i), pNoise.mu, oNoise.mu, UU1, UU2, <span class="string">'C'</span>,<span class="string">'H'</span>);
0113 
0114     Py        = C*Px_*C' + H*oNoise.cov*H';
0115     KG        = Px_ * C' * inv(Py);
0116     yh_(:,i)  = InferenceDS.hfun( InferenceDS, xh_(:,i), oNoise.mu, UU2);
0117 
0118     <span class="keyword">if</span> isempty(InferenceDS.innovation)
0119         inov(:,i) = obs(:,i) - yh_(:,i);
0120     <span class="keyword">else</span>
0121         inov(:,i) = InferenceDS.innovation( InferenceDS, obs(:,i), yh_(:,i));  <span class="comment">% inovation (observation error)</span>
0122     <span class="keyword">end</span>
0123 
0124     xh(:,i)   = xh_(:,i) + KG * inov(:,i);
0125     Px        = Px_ - KG*Py*KG';
0126 
0127     state  = xh(:,i);
0128     Pstate = Px;
0129 
0130 
0131     <span class="keyword">if</span> pNoise.adaptMethod <span class="keyword">switch</span> InferenceDS.inftype
0132     <span class="comment">%---------------------- UPDATE PROCESS NOISE SOURCE IF NEEDED --------------------------------------------</span>
0133     <span class="keyword">case</span> <span class="string">'parameter'</span>  <span class="comment">%--- parameter estimation</span>
0134         <span class="keyword">switch</span> pNoise.adaptMethod
0135         <span class="keyword">case</span> <span class="string">'anneal'</span>
0136             pNoise.cov = diag(max(pNoise.adaptParams(1) * diag(pNoise.cov) , pNoise.adaptParams(2)));
0137         <span class="keyword">case</span> <span class="string">'lambda-decay'</span>
0138             pNoise.cov = (1/pNoise.adaptParams(1)-1) * Pstate;
0139         <span class="keyword">case</span> <span class="string">'robbins-monro'</span>
0140             nu = 1/pNoise.adaptParams(1);
0141             pNoise.cov = (1-nu)*pNoise.cov + nu*KG*(KG*inov*inov')';
0142             pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
0143         <span class="keyword">otherwise</span>
0144             error(<span class="string">' [ekf]unknown process noise adaptation method!'</span>);
0145         <span class="keyword">end</span>
0146 
0147     <span class="keyword">case</span> <span class="string">'joint'</span>  <span class="comment">%--- joint estimation</span>
0148         idx = pNoise.idxArr(<span class="keyword">end</span>,:); <span class="comment">% get indexs of parameter block of combo-gaussian noise source</span>
0149         ind1 = idx(1); ind2 = idx(2);
0150         idxRange = ind1:ind2;
0151         <span class="keyword">switch</span> pNoise.adaptMethod
0152         <span class="keyword">case</span> <span class="string">'anneal'</span>
0153             pNoise.cov(idxRange,idxRange) = diag(max(pNoise.adaptParams(1) * diag(pNoise.cov(idxRange,idxRange)), pNoise.adaptParams(2)));
0154         <span class="keyword">case</span> <span class="string">'lambda-decay'</span>
0155             param_length = ind2-ind1+1;
0156             pNoise.cov(idxRange,idxRange) = (1/pNoise.adaptParams(1)-1) * Pstate(end-param_length+1:<span class="keyword">end</span>,end-param_length+1:end);
0157         <span class="keyword">case</span> <span class="string">'robbins-monro'</span>
0158             param_length = ind2-ind1+1;
0159             nu = 1/pNoise.adaptParams(1);
0160             subKG = KG(end-param_length+1:<span class="keyword">end</span>,:);
0161             pNoise.cov(idxRange,idxRange) = (1-nu)*pNoise.cov(idxRange,idxRange) + nu*subKG*(subKG*inov*inov')';
0162             pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
0163         <span class="keyword">otherwise</span>
0164             error(<span class="string">' [ekf]unknown process noise adaptation method!'</span>);
0165         <span class="keyword">end</span>
0166 
0167     <span class="comment">%--------------------------------------------------------------------------------------------------</span>
0168     <span class="keyword">end</span>; <span class="keyword">end</span>
0169 
0170 
0171 <span class="keyword">end</span>   <span class="comment">%--- for loop</span>
0172 
0173 
0174 <span class="keyword">if</span> (nargout&gt;4),
0175   InternalVariablesDS.xh_   = xh_;
0176   InternalVariablesDS.Px_   = Px_;
0177   InternalVariablesDS.yh_   = yh_;
0178   InternalVariablesDS.inov  = inov;
0179   InternalVariablesDS.Pinov = Py;
0180   InternalVariablesDS.KG    = KG;
0181 <span class="keyword">end</span>
0182</pre></div>
<hr><address>Generated on Tue 26-Sep-2006 10:36:21 by <strong><a href="http://www.artefact.tk/software/matlab/m2html/">m2html</a></strong> &copy; 2003</address>
</body>
</html>